import binascii
from datetime import datetime
from math import sqrt
import serial
import pynmea2
import os
import time
from pyproj import CRS, Transformer

# 定义串口参数
ser1_params = {
    'port': 'COM6',  # 发送指令口
    'baudrate': 115200,
    'timeout': 1
}

ser2_params = {
    'port': 'COM5',  # GPS口
    'baudrate': 115200,
    'timeout': 1
}

# 打开串口
ser1 = serial.Serial(**ser1_params)
ser2 = serial.Serial(**ser2_params)


def generate_control_command(speed_value, angle_deflection):
    angle_hex = f"{angle_deflection & 0xFFFFFFFF:08X}"  # 取低16位并转换为8位十六进制
    speed_hex = f"{-speed_value & 0xFFFFFFFF:08X}"  # 速度固定为1500，转换为8位十六进制

    # 生成控制指令
    command = f"E0010000{angle_hex}{speed_hex}"
    print(command)
    return command


def send_command(command):
    byte_command = binascii.unhexlify(command)
    ser1.write(byte_command)
    print(f"Sent command: {command}")


'''
def process_rmc_data(data):
    try:
        sentence = pynmea2.parse(data)
        if isinstance(sentence, pynmea2.RMC):
            lat = round(sentence.latitude, 6) if sentence.latitude else None
            lon = round(sentence.longitude, 6) if sentence.longitude else None
            speed = round(sentence.spd_over_grnd, 6) if sentence.spd_over_grnd else None
            return lat, lon, speed
    except (pynmea2.nmea.ChecksumError, pynmea2.ParseError) as e:
        # 忽略ChecksumError和ParseError异常
        pass
    except Exception as e:
        # 捕获并打印其他异常
        print(f"An error occurred: {e}")
    return None
'''


def process_hdt_data(data):
    try:
        sentence = pynmea2.parse(data)
        if isinstance(sentence, pynmea2.HDT):
            true_heading = float(sentence.heading)
            return true_heading
    except (pynmea2.nmea.ChecksumError, pynmea2.ParseError) as e:
        # 忽略ChecksumError和ParseError异常
        pass
    except Exception as e:
        # 捕获并打印其他异常
        print(f"An error occurred: {e}")
    return None


def calculate_distance(utm_x1, utm_y1, utm_x2, utm_y2):
    """
    计算两个UTM坐标点之间的直线距离
    """
    return sqrt((utm_x2 - utm_x1)**2 + (utm_y2 - utm_y1)**2)


def main():
    speed_value = 0
    angle_deflection = 100

    # 设置源（WGS84）和目标（UTM，广州位于UTM 50N）坐标系
    wgs84 = CRS("EPSG:4326")  # WGS 84
    utm_zone_49n = CRS("EPSG:32649")  # UTM zone 49N

    # 创建一个坐标转换器
    transformer = Transformer.from_crs(wgs84, utm_zone_49n)

    # 保存速度数据到桌面的txt文件中
    desktop_path = os.path.join(os.path.expanduser("~"), "Desktop")
    file_path = os.path.join(desktop_path, "heading_data.txt")

    # 初始化变量
    total_distance = 0.0  # 总路程
    prev_utm_x, prev_utm_y = None, None  # 上一个UTM坐标

    with open(file_path, "w") as f:
        # 运行5秒
        start_time = time.time()
        while time.time() - start_time < 10:
            while True:
                if ser2.in_waiting > 0:
                    data = ser2.readline().decode('utf-8').rstrip()
                    '''
                    lat, lon, speed = process_rmc_data(data)

                    if lat is not None:
                        # 转换为UTM坐标
                        utm_x, utm_y = transformer.transform(lat, lon)
                        timestamp = datetime.now().strftime('%H:%M:%S.%f')

                        # 如果是第一个点，仅记录坐标
                        if prev_utm_x is None:
                            prev_utm_x, prev_utm_y = utm_x, utm_y
                        else:
                            # 计算当前点与上一个点之间的距离
                            distance = calculate_distance(prev_utm_x, prev_utm_y, utm_x, utm_y)
                            total_distance += distance
                            prev_utm_x, prev_utm_y = utm_x, utm_y

                        f.write(f"Timestamp:{timestamp}\n")
                        f.write(f"Speed:{speed}, Distance:{total_distance:.2f}\n")
                    '''
                    heading = process_hdt_data(data)
                    if heading is not None:
                        timestamp = datetime.now().strftime('%H:%M:%S.%f')
                        f.write(f"Timestamp:{timestamp}\n")
                        f.write(f"heading:{heading}\n")
                    control_command = generate_control_command(speed_value, angle_deflection)
                    send_command(control_command)

    print(f"Speed data saved to {file_path}")


if __name__ == "__main__":
    try:
        main()
    finally:
        # 关闭串口
        ser1.close()
        ser2.close()
